#include "windmill.hpp"
#include <chrono>
#include <Eigen/Dense>
#include "windmill.cc"
#include <iostream>
#include <math.h>


float ideal_angle(double t1,double t2,float alpha,float C)
{
    float tmp,re;
    int k;
    tmp = -(0.785/1.884)*cos(1.884*t1+alpha)+1.305*t2 + C;
    k = tmp/(2*M_PI);
    re = tmp-k*(2*M_PI);
    if (re >M_PI)
    {
        re -= 2*M_PI;
    }
    else if (re < -M_PI)
    {
        re += 2*M_PI;
    }
    return re;
}

double angle_calculator(float MCx,float MCy,float CCx,float CCy)
{
    double re,re1;
    Eigen::Vector3d surface,forward,tmp;
    surface << 0,0,1;
    forward << 1,0,0;
    tmp << MCx-CCx,MCy-CCy,0;
    re = forward.dot(tmp)/tmp.norm();
    re1 = acos(re);
    if (forward.cross(tmp).dot(surface)<0)
    {
        re1 = -re1;
    }
    return re1;
}
int main(){

    cv::Mat shrinkedScr, greyScr, binScr, cannyDct, linePic;
    cv::Mat re;
    std::vector<std::vector<cv::Point>> contours;
    std::vector<cv::Vec4i> hierarchy;
    std::vector<cv::Point> approx;
    std::vector<int>  hull;
    int NUM = 200;
    double t_list[NUM];
    for(float tt : t_list){tt=0;}
    float CC_list_x[NUM];
    float CC_list_y[NUM];
    float MC_list_x[NUM];
    float MC_list_y[NUM];
    float distance;
    double angle_list[NUM];
    double delta_angle_list[NUM];
    int i=0;
    
    float cannyThr, FACTOR;
    double time;
    bool isMarked = false;
    double time_tmp;
    double time_init;
    cannyThr = 200;
    FACTOR = 2.5;

    cv::Moments MDct;

    cv::Point2f MC;
    cv::Point2f TC;



    WINDMILL::WindMill wm(std::chrono::system_clock::now().time_since_epoch().count() / 1e9);

    int k = NUM;
    do{
        time_tmp = std::chrono::system_clock::now().time_since_epoch().count() / 1e9;
        if (isMarked==false)
        {
            time = time_tmp;
            isMarked = true;
        }
        cv::Mat Scr = wm.getMat(time_tmp);
        
        cv::cvtColor(Scr, greyScr, cv::COLOR_BGR2GRAY);

        cv::medianBlur(greyScr, greyScr, 5);

        cv::threshold(greyScr, binScr, 20, 255, cv::THRESH_BINARY);

        cv::Canny(binScr, cannyDct, cannyThr, cannyThr*FACTOR);

        cv::findContours(cannyDct, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);

        linePic = cv::Mat::zeros(cannyDct.rows, cannyDct.cols, CV_8UC3);


        cv::Point2f CC(0, 0);


        for (int index = 0; index < contours.size(); index++){     
            if (hierarchy[index][3] != -1)
                continue;
            cv::approxPolyDP(cv::Mat(contours[index]), approx, 10, true);   
            cv::convexHull(approx, hull, false);
            
            if(cv::contourArea(approx)>1000){
                
                int temp = hierarchy[index][2];
                MDct = cv::moments(contours[hierarchy[temp][2]]);
                CC.x = CC.x + MDct.m10/MDct.m00;
                CC.y = CC.y + MDct.m01/MDct.m00;
                MC = cv::Point2f(MDct.m10/MDct.m00 , MDct.m01/MDct.m00 );
                
                // cv::drawContours(linePic, contours, index,cv::Scalar(255, 0, 0), 1, 8,hierarchy);
                if (!cv::isContourConvex(approx))
                {
                    // t_list[i] = time_tmp;
                    // CC_list_x[i] = CC.x;
                    // CC_list_y[i] = CC.y;
                    // MC_list_x[i] = MC.x;
                    // MC_list_y[i] = MC.y; 
                    // i += 1;  
                    // printf("%10f\n",t_list[i]); 
                    // circle( linePic, MC, 4, cv::Scalar(0, 0, 255), -1);
                }
                // printf("%10f\n",time_tmp);
            }
        }
        CC.x = CC.x/5;
        CC.y = CC.y/5;
        t_list[i] = time_tmp-1636018986;
        CC_list_x[i] = CC.x;
        CC_list_y[i] = CC.y;
        MC_list_x[i] = MC.x;
        MC_list_y[i] = MC.y; 
        // printf("%10f\n",time_tmp);
        i += 1;
        // circle( linePic, CC, 4, cv::Scalar(0, 255, 0), -1);
        // cv::imshow("show", linePic);
    }while (--k);
    // printf("%d\n",i);
    float sum_tmp=0;
    float angle_tmp;
    float delta_tmp;
    double current_sum = 100000000;
    double alpha_tmp = -1.001*M_PI;
    Eigen::Vector3d tmp,measure,delta,example;
    double good_alpha =-5;
    example << MC_list_x[0]-CC_list_x[0],MC_list_y[0]-CC_list_y[0],0;
    float C=angle_calculator(MC_list_x[0],MC_list_y[0],CC_list_x[0],CC_list_y[0]);
    distance = example.norm();
    while (alpha_tmp< -0.999*M_PI)
        {
            sum_tmp = 0;
            for (int j = 0; j < NUM; j++)
            {
                angle_tmp = angle_calculator(MC_list_x[j],MC_list_y[j],CC_list_x[j],CC_list_y[j]);
                delta_tmp = ideal_angle(t_list[j],t_list[j],alpha_tmp,C)-ideal_angle(t_list[0],t_list[0],alpha_tmp,C)-angle_tmp;
                sum_tmp += pow(delta_tmp,2);
                
                
                // printf("%f\n",C);
            }
            if (sum_tmp < current_sum)
                {
                    current_sum = sum_tmp;
                    good_alpha = alpha_tmp;
                }
                // printf("%f\n",good_alpha);
            alpha_tmp += 0.001;
            // printf("%f\n",sum_tmp);
        }
    float target_angle;

    printf("%f ",good_alpha);
    printf("%f\n",C);
    i = 0;
    do{
        time_tmp = std::chrono::system_clock::now().time_since_epoch().count() / 1e9;
        if (i != 1)
        {
            time_init = time_tmp;
            i += 1;
        }
        
        cv::Mat Scr = wm.getMat(time_tmp);
        
        cv::cvtColor(Scr, greyScr, cv::COLOR_BGR2GRAY);

        cv::medianBlur(greyScr, greyScr, 5);

        cv::threshold(greyScr, binScr, 20, 255, cv::THRESH_BINARY);

        cv::Canny(binScr, cannyDct, cannyThr, cannyThr*FACTOR);

        cv::findContours(cannyDct, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);

        //re = cv::Mat::zeros(cannyDct.rows, cannyDct.cols, CV_8UC3);


        cv::Point2f CC(0, 0);
        cv::Point2f TC(0.,0.);

        for (int index = 0; index < contours.size(); index++){     
            if (hierarchy[index][3] != -1)
                continue;
            cv::approxPolyDP(cv::Mat(contours[index]), approx, 10, true);   
            cv::convexHull(approx, hull, false);
            
            if(cv::contourArea(approx)>1000){
                
                int temp = hierarchy[index][2];
                MDct = cv::moments(contours[hierarchy[temp][2]]);
                CC.x = CC.x + MDct.m10/MDct.m00;
                CC.y = CC.y + MDct.m01/MDct.m00;
                MC = cv::Point2f(MDct.m10/MDct.m00 , MDct.m01/MDct.m00);
                
                //cv::drawContours(re, contours, index,cv::Scalar(255, 0, 0), 1, 8,hierarchy);
                if (!cv::isContourConvex(approx))
                {
                    circle( Scr, MC, 4, cv::Scalar(0, 0, 255), -1);
                }
                // printf("%10f\n",time_tmp);
            }
        }
        CC.x = CC.x/5;
        CC.y = CC.y/5;
        target_angle = ideal_angle(time_tmp-1636018986+0.1,time_tmp-1636018986+0.2,good_alpha,C)-ideal_angle(time-1636018986,time-1636018986,good_alpha,C);
        TC.x = CC.x + distance * cos(target_angle);
        TC.y = CC.y - distance * sin(target_angle);
        circle( Scr, CC, 4, cv::Scalar(0, 255, 0), -1);
        circle( Scr, TC, 4, cv::Scalar(255, 255,255), -1);
        cv::imshow("show", Scr);
        
    }while (cv::waitKey(1) != 'q');
    return 0;
        
}